function [c,ceq,cJac, ceqJac] = ZermeloCon(X, dt)
N = length(X)/3-1; c = []; cJac =[];
ceq = zeros(1,N*2+2); ceqJac = zeros((1+N)*2, 3*(N+1));
for i = 1:N
  xa = X((1:2) + (i-1)*3);
  ua = X(3     + (i-1)*3);
  xb = X((1:2) + (i-1+1)*3);
  xb2 = ZermeloSys(xa, ua, dt);
  ceq((1:2) +(i-1)*2) = xb-xb2;
  
  ceqJac(1+(i-1)*2, 1+(i-1)*3) = -1;
  ceqJac(1+(i-1)*2, 2+(i-1)*3) = -dt;
  ceqJac(1+(i-1)*2, 3+(i-1)*3) = dt*sin(ua);
  ceqJac(1+(i-1)*2, 4+(i-1)*3) = 1;
  ceqJac(1+(i-1)*2, 5+(i-1)*3) = 0;
  ceqJac(1+(i-1)*2, 6+(i-1)*3) = 0;
  
  ceqJac(2+(i-1)*2, 1+(i-1)*3) = 0;
  ceqJac(2+(i-1)*2, 2+(i-1)*3) = -1;
  ceqJac(2+(i-1)*2, 3+(i-1)*3) = -dt*cos(ua);
  ceqJac(2+(i-1)*2, 4+(i-1)*3) = 0;
  ceqJac(2+(i-1)*2, 5+(i-1)*3) = 1;
  ceqJac(2+(i-1)*2, 6+(i-1)*3) = 0;
end
ceqJac = ceqJac';

